#include "Communication/SubMotor.h"
#include "modules/CtrlModule/ActivePlanner/CliffFollowPlanner.h"
#include "data_struct/base/DataSet.h"
#include "data_struct/robot/MotorData.h"

namespace behavior_controller {
MotorData DataSet::motor_data;
namespace SubMotor {
static rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr motorSub;

void sub_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
    MotorData motor_data(msg->data[0], msg->data[1], msg->data[2], msg->data[3],
                         msg->data[4], msg->data[5], uint8_t(msg->data[6]));
    DataSet::motor_data = motor_data;
    DataSet::motor_data.SetTime();
}

void SuMotorInfo(rclcpp::Node::SharedPtr &node_handle) {
    motorSub = node_handle->create_subscription<std_msgs::msg::Float32MultiArray>(
            "motor", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubMotor SuMotorInfo" << std::endl;
}
} // namespace SubMotor

} // namespace behavior_controller